#include "App/task.h"
#include "App/gyro_algorithm.h"
#include "App/line_algorithm.h"
#include "App/mv_serial_data_parse.h"
#include "Components/ATK_MS901M/atk_ms901m.h"
#include "Components/algorithm.h"
#include "Components/basic.h"
#include "Components/line_patrol.h"
#include "Components/motion.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/servo.h"
#include "Components/simple_json.h"
#include <math.h>
#include <ti/driverlib/m0p/dl_interrupt.h>

#define TAG "TASK"
#define edc_send_data_to_mv uart1_send_str_with_len
atk_ms901m_attitude_data_t attitude_dat;
////////////////////TASK 3 SECTION///////////////////////////////////
////////////////////TASK 3 SECTION///////////////////////////////////
uint8_t data_to_mv[8] = {'*', '2', 0x00, 0xB4, '&'};

uint16_t user_target_angle = 180;

bool has_send_once = false;
void edc_task3_init(void)
{
  has_send_once = false;
}
void edc_task3_set_target_angle(uint16_t angle)
{
  user_target_angle = angle;
  log(TAG, "target angle = %d", user_target_angle);
}

void edc_task3_pool(void)
{
  if (has_send_once == false)
  {

    atk_ms901m_get_attitude(&attitude_dat, 100);
    uint16_t need_angle = 180 - attitude_dat.yaw;

    need_angle = need_angle - (user_target_angle - 180);
    log(TAG, "send angle = %d user_target_angle=%d", need_angle, user_target_angle);

    data_to_mv[2] = (need_angle >> 8) & 0xFF; // 高8位
    data_to_mv[3] = need_angle & 0xFF;        // 低8位
    uart1_send_str_with_len(data_to_mv, 8);
    has_send_once = true;
  }
}

////////////////////END TASK 3 SECTION///////////////////////////////

////////////////////TASK 2 SECTION///////////////////////////////////
////////////////////TASK 2 SECTION///////////////////////////////////

static bool has_s = false;
void edc_task2_init(void)
{
  has_s = false;
}
void edc_task2_pool(void)
{
  if (!has_s)
  {
    uart1_send_str("*111&");
    log(TAG, "send *111&  to mv");
    has_s = true;
  }
}

//////////////////////TASK 1 SECTION/////////////////////////////////
//////////////////////TASK 1 SECTION/////////////////////////////////
#define TURN_LINE_PATROLER_K (0.1)
#define TARGET_TURN_ANGLE (80.0f)
int car_speed = (40);
int car_turn_speed = (40);
float gyro_gain = 0.1;
float line_gain = 0.01;
int car_turn_delay = 200;
uint8_t circle_count = 0, turn_cnt = 0, target_circles = 0;
float kp = 0.2, ki = 0.00, kd = 0.0, m = 1, gain = TURN_LINE_PATROLER_K;
float delta_seconds = 0;
uint8_t delay_time = 10;
bool car_is_running = true;
control_type_t ctrl_type = CONTROL_TYPE_LINE;
uint8_t line_array[8] = {0};

bool should_check = false;

void get_delta_seconds();
bool canCarStopTurn();

void edc_task1_set_target_circle(uint8_t circle) { target_circles = circle; }

void edc_task1_init(void)
{
  motion_set_speed(car_speed);

  circle_count = 0;
  car_is_running = true;
}

int edc_task1_pool(void)
{
  static float turn_err = 0;

  if (car_is_running && target_circles > 0)
  {
    ///// handle control type///
    line_patroler_get_array(line_array);

    control_type_t cur_ctrl_type = get_line_and_gyro_err(line_array, &turn_err);

    if (cur_ctrl_type == CONTROL_TYPE_GYRO)
    {

      gain = gyro_gain;
      ctrl_type = cur_ctrl_type;

      delay(car_turn_delay);

      motor_set_mode(MOTOR_RIGHT, MOTOR_FORWARD);
      motor_set_mode(MOTOR_LEFT, MOTOR_BACKWARD);
      motor_set_torque(MOTOR_RIGHT, car_turn_speed);
      motor_set_torque(MOTOR_LEFT, car_turn_speed / 2);

      while (!canCarStopTurn())
      {
        delay(3);
        // log(TAG, "-------------turning-----------------");
      }
      motion_set_speed(car_speed);
      should_check = true;
    }

    ////motion move and turn pool/////
    get_delta_seconds();
    motion_move_pool(delta_seconds);
    motion_turn_pool(turn_err, delta_seconds, gain);

    if (should_check)
    {
      static float last_yaw = 0;
      ////get circle count/////
      atk_ms901m_get_attitude(&attitude_dat, 100);

      if (fabs(attitude_dat.yaw - last_yaw) > 40)
      {
        turn_cnt++;
        last_yaw = attitude_dat.yaw;
        should_check = false;

        log(TAG, "turn_cnt: %d", turn_cnt);
        if (turn_cnt % 4 == 0)
        {
          circle_count++;
          log(TAG, "circle_count: %d", circle_count);
          if (circle_count >= target_circles)
          {
            car_is_running = false;
            log(TAG, "-------------stop-----------------");
            motor_set_mode(MOTOR_RIGHT, MOTOR_NEUTRAL);
            motor_set_mode(MOTOR_LEFT, MOTOR_NEUTRAL);
          }
        }
      }
    }
  }

  delay(delay_time);
  return 0;
}
bool canCarStopTurn()
{
  bool ret = false; //(gyro_get_turn_err(NULL) == GYRO_TURN_DONE);

  line_patroler_get_array(line_array);
  if (line_array[3] == 1 || line_array[4] == 1)
    return true;

  return ret;
}

void get_delta_seconds()
{
  static uint32_t last_ms = 0;
  uint32_t cur_ms;
  mspm0_get_clock_ms(&cur_ms);

  delta_seconds = (cur_ms - last_ms) / 1000.0f;
  last_ms = cur_ms;
}

// void handle_mv_serial_data(recv_data_t *recv_data)
// {
//   uint8_t *str = NULL;
//   uint16_t str_len = getCurrentUart1Data(&str);
//   if (str_len && str)
//   {
//     setCurrentUART1DataLock(true);
//     parseUartData(recv_data, str, str_len);

//     setCurrentUART1DataLock(false);
//   }
// }
